Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(goal_planner): use precise distance to objects for sorting candidate paths #10296

Merged
merged 4 commits into from
Mar 24, 2025
Merged

fix(goal_planner): use precise distance to objects for sorting candidate paths #10296

merged 4 commits into from
Mar 24, 2025
+144 −47

Conversation

kosuke55
Copy link
Contributor

@kosuke55 kosuke55 commented Mar 18, 2025 •
edited
Loading

Description

Previously, we used calculateRoughDistanceToObjects to compute the maximum distance between the ego vehicle and surrounding objects when both were rotated. Based on this maximum distance, we selected a corresponding margin from the predefined collision_check_soft_margins list:

collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]  # the maximum margin when ego and objects are oriented

This margin was then used to sort the paths by priority.

image

For CAR, this approach worked reasonably well — sorting by maximum rough distance closely matched the actual distance-based sorting. However, this method proved problematic for longer vehicles like the BUS, especially when their orientation was perpendicular to the ego path. In such cases, the difference between maximum rough distance and the actual minimum distance became significant, leading to incorrect classification. As a result, nearly all paths were evaluated as having margins over 5.0m, even if they were too close.

This PR introduces a new logic:

  1. Calculate rough distance:
  • If max_distance < hard_margin, the path is considered a guaranteed collision (margin = 0.0m).
  • If min_distance > max(soft_margins) (i.e., > 5.0m), the path is considered definitely safe (margin = 5.0m).
  1. Calculate accurate Distance:
  • For cases where the rough distance falls between these two thresholds, we now compute the accurate distance between ego and the object, taking into account the orientation and footprint. The result is then used to sort the paths more reliably.

This ensures more accurate path prioritization, especially for larger like BUS or differently-oriented vehicles.

waiting for evaluator (wip
2025/03/18 https://evaluation.tier4.jp/evaluation/reports/52d79305-944b-5c4d-a119-5fc41007a5e5/?project_id=prd_jt

will updae exmaple code too

Related links

Parent Issue:

How was this PR tested?

psim + reproducer

RT1-8880-2025-03-18_14.31.47.mp4

before : goal close to object is selected

image

evaluator:
https://evaluation.tier4.jp/evaluation/reports/3d1c6b77-e2e8-5ef8-b319-0298ded5b815?project_id=prd_jt&state=failed 3750/3752

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

@github-actions github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Mar 18, 2025
@github-actions GitHub Actions
Copy link

github-actions bot commented Mar 18, 2025 •
edited
Loading

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

@kosuke55 kosuke55 added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Mar 18, 2025
@kosuke55 kosuke55 mentioned this pull request Mar 18, 2025
Merged
@kosuke55 kosuke55 force-pushed the fix/rough_margin branch 4 times, most recently from 67fa145 to 8d50f35 Compare March 21, 2025 01:54
@codecov Codecov
Copy link

codecov bot commented Mar 21, 2025 •
edited
Loading

Codecov Report

Attention: Patch coverage is 6.66667% with 28 lines in your changes missing coverage. Please review.

Project coverage is 26.21%. Comparing base (ae5eded) to head (4157d00).
Report is 2 commits behind head on main.

Files with missing lines Patch % Lines
...mon/src/utils/path_safety_checker/safety_check.cpp 10.52% 17 Missing ⚠️
...th_goal_planner_module/src/goal_planner_module.cpp 0.00% 11 Missing ⚠️
Additional details and impacted files 
@@            Coverage Diff             @@
##             main   #10296      +/-   ##
==========================================
+ Coverage   25.93%   26.21%   +0.28%     
==========================================
  Files        1383     1387       +4     
  Lines      106842   106891      +49     
  Branches    40916    40926      +10     
==========================================
+ Hits        27707    28022     +315     
+ Misses      76428    76171     -257     
+ Partials     2707     2698       -9
Flag Coverage Δ *Carryforward flag
differential 26.28% <6.66%> (?)
total 25.93% <ø> (+<0.01%) ⬆️ Carriedforward from ae5eded

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@kosuke55 kosuke55 force-pushed the fix/rough_margin branch 3 times, most recently from 252a3a7 to 5985164 Compare March 21, 2025 05:08
@kosuke55 kosuke55 force-pushed the fix/rough_margin branch 2 times, most recently from 037ac00 to 8ecf01a Compare March 24, 2025 04:47
kosuke55 added 4 commits March 24, 2025 14:41
@kosuke55
fix(goal_planner): use precise distance to objects for sorting candid…
30c1cfd 
…ate paths

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
@kosuke55
shortest_distance_from_ego_footprint_to_objects_on_path
ac1a470 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
@kosuke55
improve doxygen
1fad3cc 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

update docs

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
@kosuke55
fix build
4157d00 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
@kosuke55 kosuke55 merged commit 5ffc583 into autowarefoundation:main Mar 24, 2025
34 of 35 checks passed
@kosuke55 kosuke55 deleted the fix/rough_margin branch March 24, 2025 08:17
kosuke55 added a commit to tier4/autoware_universe that referenced this pull request Mar 27, 2025
@kosuke55
fix(goal_planner): use precise distance to objects for sorting candid…
69c4ff5 
…ate paths (autowarefoundation#10296)

* fix(goal_planner): use precise distance to objects for sorting candidate paths

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* shortest_distance_from_ego_footprint_to_objects_on_path

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* improve doxygen

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

update docs

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix build

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
@kosuke55 kosuke55 mentioned this pull request Mar 27, 2025
Merged
rej55 pushed a commit to tier4/autoware_universe that referenced this pull request Mar 27, 2025
@kosuke55
fix(goal_planner): fix margin to objects (#1933)
66262f8 
* fix(goal_planner): use precise distance to objects for sorting candidate paths (autowarefoundation#10296)

* fix(goal_planner): use precise distance to objects for sorting candidate paths

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* shortest_distance_from_ego_footprint_to_objects_on_path

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* improve doxygen

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

update docs

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix build

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat(goal_planner): expand outer collision check margin (autowarefoundation#10294)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@soblin soblin soblin approved these changes

@mkquda mkquda Awaiting requested review from mkquda mkquda is a code owner

@danielsanchezaran danielsanchezaran Awaiting requested review from danielsanchezaran danielsanchezaran is a code owner

@rej55 rej55 Awaiting requested review from rej55 rej55 is a code owner

@go-sakayori go-sakayori Awaiting requested review from go-sakayori go-sakayori is a code owner

@maxime-clem maxime-clem Awaiting requested review from maxime-clem maxime-clem is a code owner

@satoshi-ota satoshi-ota Awaiting requested review from satoshi-ota satoshi-ota is a code owner

@shmpwk shmpwk Awaiting requested review from shmpwk shmpwk is a code owner

@TakaHoribe TakaHoribe Awaiting requested review from TakaHoribe TakaHoribe is a code owner

@takayuki5168 takayuki5168 Awaiting requested review from takayuki5168 takayuki5168 is a code owner

@tkimura4 tkimura4 Awaiting requested review from tkimura4 tkimura4 is a code owner

@zulfaqar-azmi-t4 zulfaqar-azmi-t4 Awaiting requested review from zulfaqar-azmi-t4 zulfaqar-azmi-t4 is a code owner

@kyoichi-sugahara kyoichi-sugahara Awaiting requested review from kyoichi-sugahara kyoichi-sugahara is a code owner

@TomohitoAndo TomohitoAndo Awaiting requested review from TomohitoAndo TomohitoAndo is a code owner

Assignees

@soblin soblin

Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci)
Projects
Software Working Group
Status: Done
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@kosuke55 @soblin